COMMENT ⊗   VALID 00008 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00003 00002	5.8	Camera Locus Solving - the iron triangle algorithm.
C00009 00003	
C00011 00004	
C00013 00005	
C00015 00006	
C00018 00007	
C00020 00008	
C00023 ENDMK
C⊗;
5.8	Camera Locus Solving - the iron triangle algorithm.

IRON TRIANGLE CAMERA LOCUS ALGORITHM.

A  mobile  robot with only visual perception must determine where
it is going by what it sees.  Specifically, the position  of  the
robot is found relative to the position of the lens center of its
camera. The algorithm explained below if for computing the  locus
of a camera's lens center from three landmark points.

Consider  four  non-coplanar points A, B, C and L.   Let L be the
unknown camera's lens center, here after to be called the  camera
locus,  also  let  A,  B  and C be the landmark points whoes loci
either are given on a map or are found by stereo from two already
known viewing positions.

Assuming for a moment an  ideal  camera  which  can  see  all  4π
steradians  at  once, the camera can measure the angles formed by
the rays from the camera locus to the landmark points.  Let these
angles  be  called α, β and g where α is <BLC, β is <ALC and g is
<ALB.  The camera also measures whether the landmarks  appear  to
be in clockwise or counter clockwise order as seen from L. If the
landmarks are counterclockwise then B is swaped with C and β with
g.










A mechanical analog of the problem would be to position  a  rigid
triangular  piece  of sheet metal between the legs of a tripod so
that its corners touch each leg. The metal triangle is  the  same
size  as  the triangle ABC and the legs of the tripod are rigidly
held forming the angles α, β and g.










The algorithm was developed by thinking in terms of this analogy.

First,  the  triangle  edge BC is placed
between the tripod legs of angle α.  Let
a  be the length of BC, likewise b and c
are the lengths of AC and AB.



Restricting attention to the plane of B,
L & C; Consider the points L' arrived at
by sliding the  tripod  and  maintaining
contacts at B and C.



Recalling  that  in  a  circle,  a chord
subtends equal angles at any two  points
on   the   same  one  of  the  two  arcs
determined by the chord; it can be  seen
that  the  set of possible L' points lie
on a  circular  arc.  Let  this  arc  be
called  L's  arc,  which  is part of L's
circle.




Also in a circle the angle at the center
is   double    the    angle    at    the
circumference, when the rays forming the
angles meet  the  circumference  in  the
same two points.





And  the  perpendicular  bisector  of  a
chord passes  thru  the  center  of  the
chord's  circle  bisecting  the  central
angle. Let S be the distance between the
center of the circle and the chord BC.





So by trigonometric definitions
	R  =  a / 2sin(α)
	S  =  R cos(α)

The position of L on its arc in the plane BLC can be expressed in
terms  of  one  parametric  variable  w,  where  w is the counter
clockwise  angular  displacement  of  L  from  the  perpendicular
bisector such that for w=π-α L is at B and for w=α-π L is at C.












Next, spin the metal triangle about BC sweeping the vertex A thru
a circle. Let this circle be called A's circle.












Let  H  be  the  radius  of  A's circle and let D be the directed
distance between te center of A's circle and the midpoint of  BC.
By Trigonometric relations on triangle ABC:

	cos(C) = (a↑2 + b↑2 - c↑2)/2ab
	sin(C) =  sqrt(1 - cos(C)↑2)
	H = b sin(C)
  	D = b cos(C)  -  a/2


Now consider the third leg of the tripod which forms the angles β
and  g.   The  third  leg intersects the BLC plane at point L and
extends into the  appropriate  halfspace  so  that  the  landmark
points  appear to be in clockwise order as seen from L. Let A' be
the third leg's point of intersection with the  plane  containing
A's circle.




















Let the distance between the point  A'  and  the  center  of  A's
circle  less  the radius H of A's circle be called "The Gap". The
Gap's value is negative if A' falls within A's circle.


















By constructing an expression for the  value  of  the  Gap  as  a
function of the parametric variable w, a root solving routine can
find the w for  which  the  Gap  is  zero  thus  determining  the
orientation  of  the  triangle  with respect to the Tripod and in
turn the locus of the point L in space.

Lapsing  now into vector geometry place an origin at the midpoint
of BC.  Establish the unit y-vector j pointing towards the vertex
B.   Let  the  plane  BCL  be  the  xy  plane and orient the unit
x-vector i pointing into L's  halfplane.   For  right  handedness
sake set the unit z-vector k to i cross j.

In the newly defined coordinates points B, C, & L
are reached by vectors:

	B  =  (-s, +a/2, 0);
	C  =  (-s, -a/2, 0)
	L  =  (Rcos(w), Rsin(w), 0)

Introducing two unknowns xx and zz the locus
of point A' as a vector is:

	A'  =  (xx, D, zz)

The vectors corresponding to the legs of the tripod are:

	LB  =  B - L  = (-s-Rcos(w), +a/2-Rsin(w), 0)
	LC  =  C - L  = (-s-Rcos(w), -a/2-Rsin(w), 0)
	LA  =  A'- L  = (xx-Rcos(w),    D-Rsin(w), zz)

Since the third leg forms the angles β and g:

	LA . LC = |LA| |LC| cos(β)
	LA . LB = |LA| |LB| cos(g)

Solving each equation for |LA| yields:
	
	|LA| = (LA . LC)/|LC|cos(β)  =  (LA . LB)/|LB|cos(g)

Multiplying by |LB| |LC| cos(β) cos(g) gives:

	(LA . LC)|LB| cos(g) = (LA . LB)|LC| cos(β)

Expressing the vector quantites in terms of their components:

	|LB| = sqrt((-S-Rcos(w))↑2 + (+a/2-Rsin(w))↑2)
	|LC| = sqrt((-S-Rcos(w))↑2 + (-a/2-Rsin(w))↑2)

	LA . LC = (xx-Rcos(w))(-s-Rocs(w)) + (D-Rsin(w))(-a/2-Rsin(w))
	LA . LC = (xx-Rcos(w))(-s-Rocs(w)) + (D-Rsin(w))(+a/2-Rsin(w))

Substituting:

	((xx-Rcos(w))(-s-Rcos(w)) + (D-Rsin(w))(-a/2-Rsin(w)))  |LB|cos(g)
=	((xx-Rcos(w))(-s-Rcos(w)) + (D-Rsin(w))(+a/2-Rsin(w)))  |LC|cos(β)

The previous equation is linear in xx, so solving for xx:

	xx = P/Q + Rcos(w)

where

	P = (-s-Rcos(w))(|LB|cos(g) - |LC|cos(β))
	Q = (D-Rsin(w))((+a/2-Rsin(w))|LC|cos(β) 
		- (-a/2-Rsin(w))|LB|cos(g))

The unknown zz can be found from the definition of |LA|

	|LA|  =  sqrt( (xx-Rcos(w))↑2  +  (D-Rsin(w))↑2  +  zz↑2)

so 	zz    = sqrt( |LA|↑2  - (P/Q)↑2  - (D-Rsin(w))↑2)

and since:

	|LA| = (LA . LC) / |LC|cos(β)

The negative values of zz are precluded by the clockwise ordering
of  the  landmark  points. Thus the expression for the Gap can be
formed:
	
	GAP = sqrt( (XX+S)↑2 + zz↑2 )  - H

As mentioned above, when the gap is zero the  problem  is  solved
since the locus of A' then must be on A's circle, so the triangle
touches the third leg. The gap function looks like a cubic on its
interval  [α-π,π-α], and it crosses zero just once if A≠b and b≠c
and c≠a.  If the triangle ABC is isosceles  or  equilateral  then
there are two or three zero crossings respectively.

Having  found  the locus of L in the specially defined coordinate
system all that remains to do is to solve for the components of L
in  the  coordinate  system  that A, B and C were given.  This is
done by  considering  three  vector  expressions  which  are  not
dependent  on the frame of reference and do not have second order
L terms, namely:

	CA . CL
	CB . CL
	(CA x CB) . CL

Let the locus of L in the given frame of reference be (X,Y,Z) and
let the components  of  the  points  A,  B  &  C  be  (XA,YA,ZA),
(XB,YB,ZB) & (XC,YC,ZC) respectively.  Listing all four points in
both frames of reference:

A	= (xx,    D,   zz)	=   (XA, YA, ZA)
B	= (-s, +a/2,    0)	=   (XB, YA, ZA)
C	= (-s, -a/2,    0)      =   (XC, YC, ZC)
L	= (Rcos(w),Rsin(w),0)	=   ( X,  Y,  Z)

Evaluating the vector expressions which are invariant:

CA = A - C				=   (XA-XC. YA-YC, ZA-ZC)
CB = B - C = (0, a, 0)		        =   (XB-XC, YB-YC, ZB-ZC)
CL = L - C = (Rcos(w)+s,Rsin(w)+a/2,0)	=   ( X-XC,  Y-YC,  Z-ZC)

CA . CL = (xx+S)(Rcos(w)+s)+(D+a/2)(Rsin(w)+A/2)
	= (XA-XC)(X-XC) + (YA-YC)(Y-YC) + (ZA-ZC)(Z-ZC)

CB . CL	= a(Rsin(w) + a/2)
	= (XB-XC)(X-XC) + (YB-YC)(Y-YC) + (ZB-ZC)(Z-ZC)

(CA x CB) . CL = -a zz(Rcos(w) + s)
	       =  ((YA-YC)(ZB-ZC) - (ZA-ZC)(YB-YC)) (X-XC)
	        - ((XA-XC)(ZB-ZC) - (ZA-ZC)(XB-XC)) (Y-YC)
		+ ((XA-XC)(YB-YC) - (YA-YC)(XB-XC)) (Z-ZC)

The last three  equations  are  linear  equations  in  the  three
unknowns X, Y & Z which are readily isolated by Cramer's Rule.